#include "speed_control.h"

//Irgendwie in die Algorithmen integrieren
void stop_wall() {
	short distanz = 1000;
	short distanz_temp = 1000;
	byte speed = 0;
	//Lenkung geradeaus setzen
	set_servo_1_direction(SERVO_1_STEER_NULL);
	//Endlosschleife bis Abstand erreicht ist.
	set_motor_speed(MOTOR_FULL_FORWARDS);
	while (distanz > 50) {
		//Sensor auslesen
		start_US_Measure(US_5);
		_delay_ms(90);
		distanz = read_US_Data(US_5);
		if (distanz < 20) {
			distanz = 600;
		}
		//Geschwindigkeitswert berechnen
		distanz_temp = distanz / 50;
		//Voller Akku
		distanz_temp = pow(distanz_temp, 2);
		//Leerer Akku
		distanz_temp = pow(distanz_temp, 3);
		distanz_temp += MOTOR_NULL;
		if (distanz_temp > 255) {
			distanz_temp = 255;
		}
		speed = distanz_temp;
		//Geschwindigkeitswert setzen
		set_motor_speed(speed);
	}
	//Stopp
	stop_motor();
}
